/*
 * Copyright (c) 2000
 *      The Regents of the University of California.  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *      This product includes software developed by the University of
 *      California, Berkeley and its contributors.
 * 4. Neither the name of the University nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * Written by Tom Henderson
 * Version 1.2:  August 2000
 */

#ifdef STP
#include <sys/param.h>
#include <sys/systm.h> 
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/protosw.h>
#include <sys/socket.h>
#include <sys/socketvar.h>
#include <sys/errno.h>
 
#include <net/if.h>
#include <net/route.h>
   
#include <netinet/in.h>
#include <netinet/in_systm.h>
#include <netinet/ip.h>
#include <netinet/in_pcb.h>
#include <netinet/ip_var.h>
#include <netinet/stp.h>
#include <netinet/stp_var.h>

/*
 * Various routines involved with scheduling a send or poll
 */

/*
 * This function is called whenever events that change the amount of
 * eligible data that can be sent occur:
 * i) reception of a STAT
 * ii) application writes to the socket
 * iii) congestion window gets closed
 * iv) BGNAK is received with the receiver's initial window.
 *
 * We send only every tick (10 ms).  The number of eligible new packets to
 * send is stored in sp->trans_stack.  If sp->trans_stack is positive,
 * the variables sp->burst_size and sp->interval govern the sending behavior
 * every 10ms ("delayed send").  This routine sets those variables, and
 * turns on the send timer if it is off and there is data to send.
 *
 * Two mutually exclusive policies are coded:  WINDOW_CONTROL and RATE_CONTROL.
 * WINDOW_CONTROL:  If in ``low window'' state, attemps to send all
 *    eligible packets immediately.  If not in ``low window'' state, attempts
 *    to schedule packets so that the window's worth of packets is sent out
 *    smoothly across the estimated RTT of the connection.  The above is
 *    subject to both MAXBURST and RATE limitations (whichever is lower).
 *
 * RATE_CONTROL:  If (RATE < 0), attempt to send everything as soon as
 *    possible.  Otherwise, try to schedule the number of packets per tick
 *    (or ticks per packet) that gets the output rate closest to RATE.
 *    Again, the above is subject to MAXBURST limitations.  Additionally,
 *    the user is cautioned that the granularity of the rate selected (at
 *    high rates) is 1 pkt/tick, or approximately 1.2 Mb/s for full sized
 *    packets.
 */
void
stp_set_output(sp)
    register struct stpcb *sp;
{	
	struct socket *so = sp->s_inpcb->inp_socket;
	int win_pkts, pkts_in_buffer, count, rtt;
	int new_data, maxburst;

	if (sp->s_state < STP_BGN_SENT)
		return;

	/* 
 	 * Count number of eligible new transmissions and adjust 
	 * the scheduling variables (interval and burst_length) 
	 * first, determine number of outstanding packets possible.
	 */
	new_data = so->so_snd.sb_cc - sp->snd_off;
	pkts_in_buffer = new_data/sp->s_maxseg + 
	    seq_subtract(sp->snd_nxt, sp->snd_una) + 
	    (new_data % sp->s_maxseg ? 1 : 0);
	win_pkts = (WINDOW_CONTROL ? stp_window(sp) : sp->snd_wnd);
	win_pkts = min(win_pkts, pkts_in_buffer);
	/* 
	 * Win_pkts/srtt is the rate at which we should send 
	 * snd_nxt-snd_una is number of packets outstanding.
	 */
	if (win_pkts)
		sp->trans_stack = win_pkts - 
		    seq_subtract(sp->snd_nxt,sp->snd_una);
	else
		sp->trans_stack = 0;
	if (sp->trans_stack <= 0) {
		sp->trans_stack = 0;
		sp->send_pending = 0;
		return;
	}

	/* Compute maximum burst size. RATE/800 is max bytes per 10ms tick */
	if (RATE > 0)
		maxburst = min( ((RATE/800)/sp->s_maxseg), MAXBURST);
	else
		maxburst = MAXBURST;
	maxburst = (maxburst ? maxburst : 1);

	if (WINDOW_CONTROL) {
		if (stp_low_window(sp)) {
			/* Set the output to dump all packets immediately */
			sp->burst_size = sp->trans_stack;
			if (sp->burst_size > maxburst)
				sp->burst_size = maxburst;
			sp->interval = 1;
			sp->send_pending = sp->poll_pending = 1;
		} else {
			/* 
	 	 	 * Get rtt for this connection (in ms).
			 * We will space out new packets across this rtt.
		 	 */
			rtt = (sp->s_srtt ? (sp->s_srtt >> SRTT_SHIFT) : 
			    INITIAL_SRTT);
			/* 
		 	 * Calculate burst_size (number to send every tick) and 
		 	 * interval (number of ticks between bursts). 
			 * Generally, either burst_size or interval will 
			 * be >= 1, with the other = 1 
			 */
			/* 
			 * If we want to send N packets over the next K 
			 * milliseconds, we want to send N/K packets/ms.  
			 * BSD clock granularity is 10 ms = * 1 tick, so if 
			 * N/K >= 1, * send (N * 10)/K packets/tick.
			 * If N/K < 1, send a packet every K/(N * 10) ticks.
			 */
			count = (sp->trans_stack * 10) / rtt;
			if (count >= 1) { 
				/* 
				 * will send count packets every tick, bounded 
				 * by maxburst
				 */
				sp->burst_size = (count < maxburst ? count : 
				    maxburst);
				sp->interval = 1;
			} else { 
				/* Will not necessarily send every tick */
				sp->interval = rtt / (sp->trans_stack *10);
				sp->burst_size = 1;
				if (!sp->interval) 
					sp->interval = 1;
			}
			/* 
 			 * If send_pending is zero, we were not in the midst
			 * of sending something, so send at next tick.
			 */
			if (!sp->send_pending)
				sp->send_pending = 1;
		}
	} else if (RATE_CONTROL) {
		/*
		 * If RATE is negative, we should always send everything
		 * immediately.
		 */
		if (RATE < 0) {
			sp->burst_size = min(sp->trans_stack, maxburst);
			sp->interval = 1;
			sp->send_pending = 1;
			/* Poll with first burst if no polls scheduled */
			if (POLL_WITH_FIRST_BURST && !sp->poll_pending)
				sp->poll_pending = 1;
		} else {
			/*
			 * If in rate control, rate is specified in bits/s.
			 * Convert to packets/tick:
			 * RATE(b/s)/((8*sp->s_maxseg) b/packet)/(100 ticks/s).
			 */
			int pkts_per_tick, ticks_per_pkt;
			/* 
			 * These computations do (X/Y + 0.5), for more accuracy
			 */
			pkts_per_tick = (RATE + 4 * (long) sp->s_maxseg * 100)/ 
			    (8 * (long) sp->s_maxseg * 100);
			ticks_per_pkt =
			    (8 * (long) sp->s_maxseg * 100 + RATE/2)/RATE;
			if (ticks_per_pkt) {
				sp->burst_size = 1;
				sp->interval = ticks_per_pkt;
			} else {
				sp->burst_size = min(pkts_per_tick, maxburst);
				sp->interval = 1;
			}
			/* 
			 * If send_pending is zero, we were not in the midst
			 * of sending something, so send at next tick.
			 */ 
			if (!sp->send_pending)
				sp->send_pending = 1;
		}
	}
	if (!(sp->s_timer & STP_TIMER_SEND)) {
		stp_start_timer(sp, STP_TIMER_SEND, 1);
	}
}

/*
 * This function is called at the end of a data send (stp_output).  It 
 * schedules the next burst of data.  If no more data to send, turn off
 * send_pending.
 */
void 
stp_sched_output(sp)
struct stpcb *sp;
{
	struct socket *so = sp->s_inpcb->inp_socket;

	/* If no window or no data, don't schedule another send */
	if (!sp->trans_stack || (so->so_snd.sb_cc == sp->snd_off)) {
		sp->send_pending = 0;
		return;
	}
	/* otherwise, set send_pending to sp->interval and start the timer*/
	sp->send_pending = sp->interval;
	if (!(sp->s_timer & STP_TIMER_SEND))
		stp_start_timer(sp, STP_TIMER_SEND, 1);
}

/*
 * This function is called as a result of the timer expiry GTP_TIMER_SEND
 * If send_pending == 1 and/or poll_pending == 1, send something,
 * otherwise, decrement the counters and restart the timer if needed.
 */
void 
stp_send_dummy(void *arg)
{
	struct stpcb *sp;
	int temp;

	sp = (struct stpcb *) arg;

	if (sp == NULL)
		return;

	/* Turn off SEND timer */
	sp->s_timer &= ~STP_TIMER_SEND;
	
	if (1 == sp->send_pending && 1 == sp->poll_pending) {
		stp_output(sp, sp->burst_size, POLL);
	} else if (1 == sp->send_pending) {
		if (sp->poll_pending)
			sp->poll_pending--;
		stp_output(sp, sp->burst_size, NO_POLL);
	} else if (1 == sp->poll_pending) {
		if (sp->send_pending)
			sp->send_pending--;	
		stp_sndpoll(sp);
	} else {
		if (sp->send_pending)
			sp->send_pending--;	
		if (sp->poll_pending)
			sp->poll_pending--;
	}
	/* Restart timer if needed */
	if (!(sp->s_timer & STP_TIMER_SEND) && 
		(sp->send_pending || sp->poll_pending)) {
		stp_start_timer(sp, STP_TIMER_SEND, 1);
	}
}

/*
 * If either the peer's window or the congestion window is below
 * SDP_THRESHOLD segments, return true
 */
int
stp_low_window(sp)
struct stpcb *sp;
{ 
	return (stp_window(sp) < SDP_THRESHOLD);
}

/*
 * Returns the minimum (in packets) between snd_wnd and snd_cwnd 
 * Resets the congestion window if the connection has been idle for a while
 */
u_long
stp_window(sp)
struct stpcb *sp;
{
	if (time_elapsed(sp->last_send) > IDLE_RESTART * 1000) 
		stp_initialize_cwnd(sp);
	return (min(sp->snd_wnd, sp->snd_cwnd/sp->s_maxseg));
}

/*
 * This function is called from two places:
 * i) whenever a POLL is sent
 * ii) whenever a burst of data is sent
 *
 * Three cases:
 * i) if no data to send, but connection is open, start (or maintain) KEEPALIVE
 * ii) if window == 0, but data to send, PERSIST
 * iii) if poll_pending == 0, schedule a new POLL according to the policy
 * NOTE:  This function will not touch an already scheduled POLL
 */
void 
stp_sched_poll(sp)
struct stpcb *sp;
{
	struct socket *so = sp->s_inpcb->inp_socket;
	int rto, poll_ticks;

	/* if nothing to send, start keepalive timer if not already running */
	if ((sp->snd_nxt == sp->snd_una) && !so->so_snd.sb_cc) {
		if (!(sp->s_timer & STP_TIMER_KEEPALIVE)) {
			stp_killtimers(sp); 
			stp_start_timer(sp, STP_TIMER_KEEPALIVE, NULL);
		}
		sp->poll_pending = 0;
		return;
	} else if (sp->s_timer & STP_TIMER_KEEPALIVE)
		stp_cancel_timer(sp, STP_TIMER_KEEPALIVE);
	/* if window == 0, but data to send, and no urgent data, persist */
	if ((sp->snd_nxt == sp->snd_una) && so->so_snd.sb_cc && 
			!sp->snd_up && !sp->snd_wnd) {
		if (!(sp->s_timer & STP_TIMER_PERSIST)) {
			stp_start_timer(sp, STP_TIMER_PERSIST, NULL);
			return;
		}
	} else if (sp->s_timer & STP_TIMER_PERSIST)
		stp_cancel_timer(sp, STP_TIMER_PERSIST);
	
        /*
         * rto-- like TCP's retransmission timeout
         *       should be slightly larger than the smoothed rtt
         *       to avoid POLLing again if STAT is a little slow in returning
         */
	rto = (sp->s_srtt ? ((sp->s_srtt + sp->s_rttvar) >> SRTT_SHIFT) : 
	    INITIAL_SRTT);
	if (!sp->poll_pending) {
		/*
		 * Policy-- Poll ``POLLS_PER_RTT'' times per RTT
		 *          subject to the MIN_POLL_INTERVAL
		 */
		if ((rto/POLLS_PER_RTT) < MIN_POLL_INTERVAL)
			poll_ticks = MIN_POLL_INTERVAL/MS_PER_TICK;
		else
			poll_ticks = (rto/POLLS_PER_RTT)/MS_PER_TICK;
		sp->poll_pending = poll_ticks;
		/*
		 * Now apply any backoffs
		 */
		if (sp->snd_backoff)
			sp->poll_pending <<= sp->snd_backoff;
		else 
			sp->poll_pending <<= (sp->no_stat_response / 
			    STP_POLL_BACKOFF);
		/*
		 * Sanity check
		 */
		if (sp->poll_pending < 1 || sp->poll_pending > MAX_POLL_TICKS) {
			printf("Error:  poll_pending set to %d; poll_ticks %d; no_stat_resp %d, snd_backoff %d\n", sp->poll_pending, poll_ticks, sp->no_stat_response, sp->snd_backoff);
			sp->poll_pending = 1;
		}
        }
	if (!(sp->s_timer & STP_TIMER_SEND)) {
		stp_start_timer(sp, STP_TIMER_SEND, 1);
	}
}

/*
 * This function called whenever poll_pending is set to a non-zero value.
 * It backs off the poll timer when needed.  "val" is in ticks 
 */
void 
stp_set_poll_pending(sp, val)
struct stpcb *sp;
int val;
{
	sp->poll_pending = val;
	/* Backoff */
	if (sp->snd_backoff) {
		/* Have been stuck on the same retransmission */
		sp->poll_pending <<= sp->snd_backoff;
	} else {
		sp->poll_pending = sp->poll_pending <<
		    (sp->no_stat_response / STP_POLL_BACKOFF);
	}
	/* Sanity check */
#define MAX_POLL_TICKS 1000
	if (sp->poll_pending < 1 || sp->poll_pending > MAX_POLL_TICKS) {
		DBG(printf("Error:  poll_pending set to %d\n", sp->poll_pending););
		sp->poll_pending = 1;
	}
}

/*
 * This is the handler for the BGN_TIMER and END_TIMER.  It is
 * used to retransmit BGN's and END's until we get the corresponding
 * BGNAK's and ENDAK's or give up.
 */
void 
stp_sendcc_dummy(void *arg)
{
	struct stpcb *sp;
	int to_value, error;

	sp = (struct stpcb *) arg;
	
	if (sp == NULL)
		return;

	/* Both BGN and END timers should not be running */
	if((sp->s_timer & STP_TIMER_BGN)&&(sp->s_timer & STP_TIMER_END))
		DBG(printf("Error, BGN and END timers both running\n"););

	if (sp->cc_count) {
		if (sp->cc_count >= MAX_CC_ATTEMPTS) {
			/* p. 843 */
			sp = stp_drop(sp, sp->s_softerror ? 
			    sp->s_softerror : ETIMEDOUT);
			return;
		}
		if (sp->s_srtt)	
			to_value = ((sp->s_srtt >> SRTT_SHIFT) << 
			    sp->cc_count)/MS_PER_TICK;
		else 
			to_value = (INITIAL_SRTT << sp->cc_count)/MS_PER_TICK;
		switch (sp->s_state) {
		case STP_BGN_SENT:
			error = stp_sndcc_open(sp, STP_PDU_BGN);
			stp_start_timer(sp, STP_TIMER_BGN, to_value);
			break;
		case STP_BGN_DATA_SENT:
		case STP_BGN_DATA_END_SENT:
			error = stp_sndcc_open(sp, STP_PDU_BGN | STP_PDU_SD |
			    STP_PDU_POLL);
			stp_start_timer(sp, STP_TIMER_BGN, to_value);
			break;
		case STP_HALF_CLOSE_PENDING:
		case STP_FULL_CLOSE_PENDING:
		case STP_LAST_ACK:
			error = stp_sndcc_close(sp, STP_PDU_END);
			stp_start_timer(sp, STP_TIMER_END, to_value);
			break;
		}
		sp->cc_count++;
	} else {
		/* Do nothing */;
		DBG(printf("Forgot to cancel this timeout timer %x state %x\n", sp->s_timer, sp->s_state););
	}
}

/*
 * Routines involved with the actual data transmission
 */

/* 
 * This function called to send NEW data.  Retransmissions are handled by 
 * stp_process_list() based on USTAT or STAT reception.  "force" counts the 
 * number of packets to try to send now.  "poll" stimulates SDP with last 
 * packet. Urgent data is sent in its own special packet "SDP_URG".
 */
int
stp_output(sp, force, poll)
    register struct stpcb *sp;
	int force;
	int poll;
{	
	struct socket *so = sp->s_inpcb->inp_socket;
	struct mbuf *m;
	struct stpiphdr *si;
	long len;
	u_long type_field, timestamp, *dataptr;
	int snd_nxt_mod, hdrlen, error, i;
	flag sending_end, sending_urg;

	if (sp->s_state < STP_BGN_SENT || sp->s_state > STP_CLOSE_RECV)
		return (0);

	force = (force < sp->trans_stack ? force : sp->trans_stack); 
	if (force < 1) { 
		/* nothing to send; don't schedule another right now */
		if (poll)
			stp_sndpoll(sp);
		return (0);
	}

	while (force--) {
		sending_urg = sending_end = FALSE;
		snd_nxt_mod = sp->snd_nxt % MAXBUFFER_STP;
		len = so->so_snd.sb_cc - sp->snd_off;
		if (len > sp->s_maxseg)
			len = sp->s_maxseg;
		if (len < 0) {	
			/* Shouldn't happen */
			DBG(printf("Length goes negative\n"););
			len = 0;
		}

		if (len == 0) {
			if (poll) 
				stp_sndpoll(sp);
			return (0);
		}
		if (len < TINYGRAM) {
			/*
			 * Silly window avoidance.  If TCP_NODELAY has been
			 * called (Nagle disabled), then always send the
			 * tinygram.  Otherwise, avoid sending the PDU
			 * unless the urgent pointer is set.
			 * Note:  This piece of code, if better tuned, may
			 * improve Telnet performance over satellite links
			 * (but it will likely still be lousy in any case)
			 */ 
			if (!sp->stp_nodelay && sp->tinygram_out &&
			    !sp->snd_up) {
				if (poll)	
					stp_sndpoll(sp);
				return (0);
			} 
			sp->tinygram_out = TRUE;
		}
		/* 
		 * Urgent data:  If in urgent mode and the next byte of data 
		 * is the urgent byte, send it as an SDP_URG.  If in urgent 
		 * mode, and if sending a full sized segment would move us 
		 * past the urgent pointer, send only up to the urgent byte
		 */
		if ((sp->snd_up - sp->snd_off) > 0) {
			ASSERT(sp->snd_up > 0);
			if ((sp->snd_up - sp->snd_off) == 1) {
				sending_urg = TRUE;
				len = 1;
			} else if ((len + sp->snd_off) >= sp->snd_up) {
				len = sp->snd_up - sp->snd_off - 1;
			}
		}

		hdrlen = sizeof(struct stpiphdr);		
		/* Set type field */
		type_field = STP_PDU_SD;
		if (sending_urg)
			type_field |= STP_PDU_URG | STP_PDU_POLL;
		/* 
		 * If this is the last one we are sending in this burst AND
		 * the sending process has closed AND this is the last PDU in
		 * the send buffer, turn off poll, and turn on sending_end.
		 */
		if ( !force && (sp->shutdown_pending || sp->disc_pending) &&
			(so->so_snd.sb_cc == (sp->snd_off + len)) ) {
			sending_end = TRUE;
		}
		sp->data_counter++;
		if (!force && !sending_urg) {
			/* 
		 	 * If sending process has closed, use this opportunity 
			 * to piggyback the end if it is the last PDU to send.
		 	 */
			if (sending_end) {
				type_field = STP_PDU_SD | STP_PDU_POLL |
				   STP_PDU_END;
				poll = TRUE;
			} else {
				/*
				 * POLL whenever we have sent 1/STP_POLL_LIMITth
				 * of the max socket buffer since
				 * last polling.  The factor of 2 accounts
				 * Linux setting sndbuf to twice the sockopt
				 * maxbuffer setting,then using only half of it
				 */
				if (poll || sp->data_counter >=
					((so->so_snd.sb_hiwat/sp->s_maxseg)/4/
					STP_POLL_LIMIT) ) {
					type_field = STP_PDU_SD | STP_PDU_POLL;
					poll = TRUE;
				}
			}
		}
		m = stp_alloc_packet(sp, type_field, sp->snd_nxt); 
		if (NULL == m) {
			error = ENOBUFS;
			goto out;
		}
		si = mtod(m, struct stpiphdr *);

		/* Add timestamp if last one is a SDP, or if urg.data is sent */
		if ((!force && poll) || sending_urg) {
			timestamp = current_time();
			dataptr = (u_long *) (mtod(m, caddr_t) + hdrlen);
			*dataptr = htonl(timestamp);
			m->m_len += 4;
			hdrlen += 4;
			sp->poll_pending = 0;
			sp->data_counter = 0; 
		}
		/* 
		 * At this point, m_data points to start of IP packet.  hdrlen
		 * counts the number of bytes after which data can be inserted.
		 */
		if (len <= MHLEN - hdrlen - max_linkhdr) {
			m_copydata(so->so_snd.sb_mb, sp->snd_off, (int) len, 
			    mtod(m, caddr_t)+ hdrlen);	
			m->m_len += len;
		} else {
			m->m_next = m_copy(so->so_snd.sb_mb, sp->snd_off, 
			    (int) len);
			if (m->m_next == 0)
				len = 0;
		}
		/* Save record of transmission */
		sp->trans_buffer[snd_nxt_mod].seqno = sp->snd_nxt;
		sp->trans_buffer[snd_nxt_mod].num_bytes = len;
		sp->trans_buffer[snd_nxt_mod].timesent = current_time();
		sp->trans_buffer[snd_nxt_mod].num_trans = 1; 
		sp->snd_off += len;
		if (len > sp->s_maxseg)
			DBG(printf("snd_off being corrupted; len:%d\n", len););

		sp->snd_nxt = SEQ_INC(sp->snd_nxt);

		stp_finish_packet(sp, si, m, hdrlen, len);

		/* mbuf data pointer shouldn't point beyond link layer space */
		error = ip_output(m, sp->s_inpcb->inp_options, 
		    &sp->s_inpcb->inp_route, so->so_options & SO_DONTROUTE, 0);

		/* Decrement transmission stack */
		sp->trans_stack--;
		sp->last_send = current_time();

		if (error) {
out:
			if (error == ENOBUFS) {
				stp_quench(sp->s_inpcb, 0);
				/* XXX Potential bug here if return w/o 
				 * checking for a poll? 
				 */
				return (0);
			}
		}	
	} /* while (force) */

	ASSERT(sp->trans_stack >= 0);

	/* schedule next output */
	stp_sched_output(sp);
	stp_sched_poll(sp);

	return (0);
}

/*  
 *  This routine finds the data corresponding to packet number "seqno"
 *  and retransmits the packet.
 *  Function returns -1 if an error was detected.
 */
int 
stp_retransmit(sp, seqno)
	register struct stpcb *sp;
	u_long seqno;
{   
	struct socket *so = sp->s_inpcb->inp_socket;
	struct mbuf *m;
	struct stpiphdr *si;
	u_long i, seqno_field, type_field;
	int hdrlen, error;
	int i_mod, seqno_mod;
	int offset = 0;
	int tries;
	long len;
     
	seqno_mod = seqno % MAXBUFFER_STP;
	/* Find the offset in the send transmit buffer. */
	for (i = sp->snd_una; seq_lt(i, seqno); i = SEQ_INC(i)) {
		i_mod = i % MAXBUFFER_STP;
		offset += sp->trans_buffer[i_mod].num_bytes;
	}   
	len = sp->trans_buffer[seqno_mod].num_bytes;
	hdrlen = sizeof(struct stpiphdr);
	type_field = STP_PDU_SD;
	/* 
	 * If we previously sent an END_SD, and we have to retransmit it, send
	 * another END_SD.
	 */
	if (sp->end_sent && seqno == SEQ_DEC(sp->snd_nxt))
    		type_field = (STP_PDU_SD | STP_PDU_POLL | STP_PDU_END);
	m = stp_alloc_packet(sp, type_field, seqno); 
	if (NULL == m) {    
		error = ENOBUFS;    
		goto out;
	}       
	si = mtod(m, struct stpiphdr *);
    
	if (len <= MHLEN - hdrlen - max_linkhdr) {
		/* sp->snd_off indicates where the sender is w.r.t. snd_nxt */
		m_copydata(so->so_snd.sb_mb, offset, (int) len, 
		    mtod(m, caddr_t)+ hdrlen);  
		m->m_len += len;
	} else {
		m->m_next = m_copy(so->so_snd.sb_mb, offset, (int) len);
		if (m->m_next == 0)
			len = 0;
	}       
	/* Save record of transmission */
	sp->trans_buffer[seqno_mod].timesent = current_time();
	sp->trans_buffer[seqno_mod].num_trans += 1;
    
	/* Upon 3rd transmission of a packet, start backing off retxs */
	/* XXX get rid of magic number */
	tries = TRIES_BEFORE_BACKOFF - 1;
	if ((sp->trans_buffer[seqno_mod].num_trans > tries) &&
	    (sp->snd_backoff < (sp->trans_buffer[seqno_mod].num_trans - 
	    tries)) ) {
		sp->snd_backoff = sp->trans_buffer[seqno_mod].num_trans - tries;
		sp->snd_backoff = (sp->snd_backoff > MAXBACKOFF ? MAXBACKOFF :
		    sp->snd_backoff);
		DBG(printf("Backing off: sp->snd_backoff %d\n", sp->snd_backoff););
	}
	if (sp->trans_buffer[seqno_mod].num_trans > STP_REROUTE) 
		/* ask IP to find a new route */
		in_losing(sp->s_inpcb);
	if (sp->trans_buffer[seqno_mod].num_trans > STP_MAXRETRANS) {
		/* XXX Is this the right error value?  Do we want to reset? */
		DBG(printf("Over max retransmissions %d, stuck on %x\n", sp->trans_buffer[seqno_mod].num_trans, seqno););
		stp_drop(sp, EINVAL);
		return STP_CONN_DROPPED;
	}
    
	stp_finish_packet(sp, si, m, hdrlen, len);
    
	/* mbuf data pointer should not be pointing beyond link layer space */
	error = ip_output(m, sp->s_inpcb->inp_options, &sp->s_inpcb->inp_route,
	    so->so_options & SO_DONTROUTE, 0);

	if (error) {
out:    
		if (error == ENOBUFS) {
			stp_quench(sp->s_inpcb, 0);
			return (0);
		}
	}
	return (0);
}       


/* 
 * When there is no pcb for a received packet, this function generates a
 * response.  The value for type_field (either ENDAK or RST) is passed in.
 */
void
stp_reset(m, si)
	struct mbuf *m;
	struct stpiphdr *si;
{
	struct route *ro = 0;
	u_long seqno_field = 0;

	/* 
	 * No need to allocate another packet.  Free any subsequent mbufs,
	 * assign mbuf data pointer to the stpiphdr pointer, overwrite
	 * sequence number field, exchange addresses and ports, and
	 * zero out checksum field
	 */
	m_freem(m->m_next);
	m->m_next = 0;
	m->m_data = (caddr_t) si;
#define xchg(a,b,type) { type t; t=a; a=b; b=t;}
	xchg(si->si_dst.s_addr, si->si_src.s_addr, u_long);
	xchg(si->si_dport, si->si_sport, u_short);
#undef xchg
	seqno_field = (seqno_field | (STP_PDU_RST << 24));
	si->si_seq = htonl(seqno_field);
	si->si_sum = 0;
	/* Leave si->si_inc as it is */
	m->m_len = sizeof (struct stpiphdr);
	m->m_pkthdr.rcvif = (struct ifnet *) 0;
		
	/* stp_finish_packet() can't be called here because there is no pcb */
	si->si_len = htons((u_short) (sizeof(struct stphdr)));
	si->si_sum = stp_cksum(m, sizeof (struct stphdr));
	m->m_pkthdr.len = sizeof(struct stpiphdr);
	((struct ip *) si)->ip_len = m->m_pkthdr.len;
	((struct ip *) si)->ip_ttl = ip_defttl;

	/* mbuf data pointer should not be pointing beyond link layer space */
	(void) ip_output(m, NULL, ro, 0, NULL);
}

/*
 * This function called to output one of the following PDUs:
 * BGN, BGN_SD, BGN_SD_END, BGNAK, BGNAK_STAT, BGNAK_ENDAK
 */
int
stp_sndcc_open(sp, pkttype)
	register struct stpcb *sp;
	u_long pkttype;
{
	struct socket *so = sp->s_inpcb->inp_socket;
	struct mbuf *m;
	struct stpiphdr *si;
	int hdrlen;
	int error, index;
	u_long *dataptr;
	int datalen = 0;	/* amount of user data appended */
	int data_offset = 0;	/* offset from header */
	long len = 0;		/* total length of STP packet */

	u_long timestamp = current_time(); 
	
	/* First, determine packet type to send */
	if (IS_SD_PDU(pkttype)) { /* Data included */
		if (so->so_snd.sb_cc) {
			data_offset = 8; /* window and timestamp */
			index = sp->snd_iss % MAXBUFFER_STP;
			if (sp->snd_nxt == sp->snd_iss) 
				datalen = min(sp->s_maxseg - data_offset,
				    so->so_snd.sb_cc);    
			else {
				/* Have sent data before (retransmission) */
				datalen = sp->trans_buffer[index].num_bytes;	
						
			}
		} else {
			/* No data to send (log protocol error?) */
			pkttype &= ~STP_PDU_SD;
		}
		/*
		 * Check if datalen = length of send buffer
		 * and if there is disconnect or shutdown
		 * pending.  If so, change pkttype to *_END
		 */
		if (datalen == so->so_snd.sb_cc && IS_SD_PDU(pkttype) &&
		    (sp->shutdown_pending || sp->disc_pending))
			pkttype |= STP_PDU_END;
	}
	/* Next, allocate packet and fill in common fields.  */
	m = stp_alloc_packet(sp, pkttype, sp->snd_iss);
	if (NULL == m) {	
		stp_quench(sp->s_inpcb, 0);
		return (0);
	}
	si = mtod(m, struct stpiphdr *);
	hdrlen = sizeof (struct stpiphdr);

	/* Add send window (4 bytes) */
	sp->rcv_wnd = sbspace(&so->so_rcv) / sp->s_peermaxseg;
	dataptr = (u_long *) (mtod(m, caddr_t) + hdrlen);
	*dataptr = htonl(sp->rcv_wnd);
	len  += 4;
	m->m_len += 4;

	/* Add timestamp (4 bytes) */
	dataptr++;
	if (IS_BGNAK_PDU(pkttype))  /* Some type of BGNAK */
		*dataptr = htonl(sp->bgnak_timestamp);
	else
		*dataptr = htonl(timestamp);
	len += 4;
	m->m_len += 4;

	/* Append data */
	if (data_offset) {
   		if (datalen <= MHLEN - hdrlen - max_linkhdr - data_offset) {
        		m_copydata(so->so_snd.sb_mb, 0, (int) datalen, 
			    mtod(m, caddr_t) + hdrlen + data_offset);
        		m->m_len += datalen;
			len += datalen;
    		} else {
        		m->m_next = m_copy(so->so_snd.sb_mb, 0, (int) datalen);
			len += datalen;
			if (m->m_next == 0)
				len = 0;
    		}
		sp->trans_buffer[index].seqno = sp->snd_iss;
		sp->trans_buffer[index].num_bytes = datalen;
		sp->trans_buffer[index].timesent = current_time();
		sp->trans_buffer[index].timesent += 1;
		if (sp->snd_nxt == sp->snd_iss) {
			sp->snd_nxt = SEQ_INC(sp->snd_nxt);
			sp->snd_off += datalen;
			if (len > sp->s_maxseg)
				DBG(printf("snd_off corrupted; datalen:%d\n", datalen););
		}
	}

	stp_finish_packet(sp, si, m, hdrlen, len);

	/* mbuf data pointer should not be pointing beyond link layer space */
	error = ip_output(m, sp->s_inpcb->inp_options, &sp->s_inpcb->inp_route,
	    so->so_options & SO_DONTROUTE, 0);
	return error;
}
/*
 * This function called to output one of the following PDUs:
 * END (which becomes a POLL_END automatically) or ENDAK.
 */
int
stp_sndcc_close(sp, pkttype)
	register struct stpcb *sp;
	u_long pkttype;
{
	struct socket *so = sp->s_inpcb->inp_socket;
	struct mbuf *m;
	struct stpiphdr *si;
	int hdrlen;
	int error, index;
	u_long *dataptr;
	long len = 0;

	if (IS_END_PDU(pkttype)) {
		stp_sndpoll(sp); /* Send a POLL_END */
		return 0;
	}
	/* The following will only be executed for ENDAKs */
	m = stp_alloc_packet(sp, pkttype, sp->snd_nxt);
	if (NULL == m) {	
		stp_quench(sp->s_inpcb, 0);
		return (0);
	}
	si = mtod(m, struct stpiphdr *);
	hdrlen = sizeof (struct stpiphdr);

	stp_finish_packet(sp, si, m, hdrlen, len);

	/* mbuf data pointer should not be pointing beyond link layer space */
	error = ip_output(m, sp->s_inpcb->inp_options, &sp->s_inpcb->inp_route,
	    so->so_options & SO_DONTROUTE, 0);
	return error;
}

/* Send a RST PDU for this particular connection instance */
int
stp_sndrst(sp)
    register struct stpcb *sp;
{
	struct socket *so = sp->s_inpcb->inp_socket;
	struct mbuf *m;
	struct stpiphdr *si;
	int error;
	long len = 0;

	u_long timestamp = current_time(); 

	m = stp_alloc_packet(sp, STP_PDU_RST, sp->snd_nxt); 
	if (NULL == m) {	
		/* XXX what to do if allocation fails here? */
		stp_quench(sp->s_inpcb, 0);
		return (0);
	}
	si = mtod(m, struct stpiphdr *);
		
	si->si_sum = stp_cksum(m, sizeof (struct stphdr));

	m->m_pkthdr.len = sizeof (struct stpiphdr);
	((struct ip *) si)->ip_len = m->m_pkthdr.len;
	((struct ip *) si)->ip_ttl = sp->s_inpcb->inp_ip_ttl;
	((struct ip *) si)->ip_tos = sp->s_inpcb->inp_ip_tos;

	/* mbuf data pointer should not be pointing beyond link layer space */
	error = ip_output(m, sp->s_inpcb->inp_options, &sp->s_inpcb->inp_route,
	    so->so_options & SO_DONTROUTE, 0);

	return error;
}

int
stp_sndpoll(sp)
	register struct stpcb *sp;
{
	struct socket *so = sp->s_inpcb->inp_socket;
	struct mbuf *m;
	struct stpiphdr *si;
	long len;
	int hdrlen;
	u_long type_field;
	u_long timestamp;
	int error;
	u_long *dataptr;

	len = 0;
	timestamp = current_time();
	type_field = STP_PDU_POLL;
	if ((sp->disc_pending || sp->shutdown_pending) &&
	    sp->snd_nxt == sp->snd_una &&
	    so->so_snd.sb_cc == 0)
		type_field |= STP_PDU_END;
	if (sp->snd_up)
		type_field |= STP_PDU_URG;
	hdrlen = sizeof (struct stpiphdr);

	m = stp_alloc_packet(sp, type_field, sp->snd_nxt); 
	if (NULL == m) {	
		/* If allocation fails, put connection in slow start */
		stp_quench(sp->s_inpcb, 0);
		return (0);
	}
	si = mtod(m, struct stpiphdr *);
		
	/* Add timestamp */
	dataptr = (u_long *) (mtod(m, caddr_t) + hdrlen);
	*dataptr = htonl(timestamp);
	len += 4;
	m->m_len += 4;
	
	stp_finish_packet(sp, si, m, hdrlen, len);

	/* mbuf data pointer should not be pointing beyond link layer space */
	error = ip_output(m, sp->s_inpcb->inp_options, &sp->s_inpcb->inp_route,
	    so->so_options & SO_DONTROUTE, 0);

	sp->data_counter = 0;
	sp->poll_pending = 0;
	stp_sched_poll(sp);

	return error;
}
	
int
stp_sndstat(sp, timestamp)
	register struct stpcb *sp;
	u_long timestamp;
{
	struct socket *so = sp->s_inpcb->inp_socket;
	struct mbuf *m;
	struct stpiphdr *si;
	int error;
	long win;
	u_long *dataptr;
	u_long i;
	int i_mod;
	int num_elements = 0;
	long len = 0; 
	int hdrlen = sizeof (struct stpiphdr);

	m = stp_alloc_packet(sp, STP_PDU_STAT, sp->rcv_nxt); 
	if (NULL == m) {    
		/* If allocation fails, put connection in slow start */
		stp_quench(sp->s_inpcb, 0);
		return (0);
	}
	si = mtod(m, struct stpiphdr *);

	/* Add timestamp */
	dataptr = (u_long *) (mtod(m, caddr_t) + hdrlen);
	sp->rcv_wnd = sbspace(&so->so_rcv) / sp->s_peermaxseg;
	*dataptr = htonl(sp->rcv_wnd);
	len += 4;
	m->m_len += 4;
	dataptr++;

	*dataptr = htonl(timestamp);
	len += 4;
	m->m_len += 4;
	
	/* Build stat list (first check if one is needed) */
	/* For now, limit size of STAT to fit in mbuf */
	if (seq_lt(sp->rcv_nxt, sp->rcv_max)) {
		i = sp->rcv_nxt;
		while (seq_lt(i, sp->rcv_max) && num_elements <= MAXSTAT) {
			i_mod = i % MAXBUFFER_STP;
			while (seq_lt(i, sp->rcv_max) && 
			    sp->recv_buffer[i_mod].received) {
				i = SEQ_INC(i);	
				i_mod = i % MAXBUFFER_STP;
			}
			/* Add i to list */
			dataptr++;
			*dataptr = htonl(i);
			len += 4;
			m->m_len += 4;
			num_elements++;
			if (seq_lt(i, sp->rcv_max) && num_elements < MAXSTAT) {
				while (seq_lt(i, sp->rcv_max) && 
				    !sp->recv_buffer[i_mod].received) {
					i = SEQ_INC(i);	
					i_mod = i % MAXBUFFER_STP;
				} /* while */
				/* Add i to list */
				dataptr++;
				*dataptr = htonl(i);
				len += 4;
				m->m_len += 4;
				num_elements++;
			} /* if */
		} /* while (i < sp->rcv_max) */
	} /* if (stat list needed) */

	stp_finish_packet(sp, si, m, hdrlen, len);

	/* mbuf data pointer should not be pointing beyond link layer space */
	error = ip_output(m, sp->s_inpcb->inp_options, &sp->s_inpcb->inp_route,
	    so->so_options & SO_DONTROUTE, 0);

	return error;
}
	
int
stp_sndustat(sp)
	register struct stpcb *sp;
{
	struct socket *so = sp->s_inpcb->inp_socket;
	struct mbuf *m;
	struct stpiphdr *si;
	long len;
	int hdrlen;
	int error;
	u_long *dataptr;
	u_long list1 = sp->ustat_list1;
	u_long list2 = sp->ustat_list2;

	len = 0; 
	hdrlen = sizeof (struct stpiphdr);

	m = stp_alloc_packet(sp, STP_PDU_USTAT, sp->rcv_nxt); 
	if (NULL == m) {	
		/* If allocation fails, put connection in slow start */
		stp_quench(sp->s_inpcb, 0);
		return (0);
	}
	si = mtod(m, struct stpiphdr *);
		
	/* Add list elements */
	dataptr = (u_long *) (mtod(m, caddr_t) + hdrlen);
	*dataptr = htonl(list1);
	len += 4;
	m->m_len += 4;
	dataptr += 1;
	*dataptr = htonl(list2);
	len += 4;
	m->m_len += 4;
	
	stp_finish_packet(sp, si, m, hdrlen, len);

	/* mbuf data pointer should not be pointing beyond link layer space */
	error = ip_output(m, sp->s_inpcb->inp_options, &sp->s_inpcb->inp_route,
	    so->so_options & SO_DONTROUTE, 0);

	return error;
}

/* 
 * Helper function:  Allocate packet and fill in generic fields. 
 * Return NULL if allocation fails.  Otherwise, return mbuf with STP 
 * generic fields (port numbers, type field, seqno, instance number) filled in
 */
struct mbuf *
stp_alloc_packet(sp, type_field, seqno_field)
	register struct stpcb *sp;
	u_long type_field;
	u_long seqno_field;
{
	struct mbuf *m;
	struct stpiphdr *si;

	MGETHDR(m, M_DONTWAIT, MT_HEADER); /* Allocate mbuf */
	if (NULL == m)
		return m;
	m->m_data += max_linkhdr; /* advance data ptr. beyond ethernet hdr. */	
	m->m_len = sizeof (struct stpiphdr);	/* length of STP/IP header */

	m->m_pkthdr.rcvif = (struct ifnet *) 0;
	si = mtod(m, struct stpiphdr *);
	if (sp->s_template == 0)
		panic("stp_alloc_packet");
	/* Copy template of IP and STP headers into mbuf */
	bcopy((caddr_t) sp->s_template, (caddr_t) si, sizeof(struct stpiphdr));
	/* 
	 * Fill in fields.  The sequence number field contains a 24 bit
	 * `instance' number and a 8 bit `type' field in the highest order bits.
	 */
	seqno_field = (seqno_field | (type_field << 24));
	si->si_seq = htonl(seqno_field);
	si->si_inc = htons(sp->s_instance);

	return m;
}

/* Helper function to finish off filling in some mbuf fields. */
void
stp_finish_packet(sp, si, m, hdrlen, len)
	register struct stpcb *sp;
	struct stpiphdr *si;
	struct mbuf *m;
	int hdrlen;
	long len;
{
	/* Length = STP PDU length; Checksum covers STP PDU only */
	si->si_len = htons((u_short) (hdrlen + len - sizeof (struct ip)));
	si->si_sum = stp_cksum(m, hdrlen + len - sizeof (struct ip));

	m->m_pkthdr.len = hdrlen + len;
	((struct ip *) si)->ip_len = m->m_pkthdr.len;
	((struct ip *) si)->ip_ttl = sp->s_inpcb->inp_ip_ttl;
	((struct ip *) si)->ip_tos = sp->s_inpcb->inp_ip_tos;
}
#endif
